2D/3D localization and pose estimation of harness cables using a configurable structure representation for robot operations

ABSTRACT

A robot is made to recognize and manipulate different types of cable harnesses in an assembly line. This is achieved by using a stereo camera system to define a 3D cloud of a given cable harness. Pose information of specific parts of the cable harness are determined from the 3D point cloud, and the cable harness is then re-presented as a collection of primitive geometric shapes of known dimensions, whose positions and orientations follow the spatial position of the represented cable harness. The robot can then manipulate the cable harness by using the simplified representation as a reference.

BACKGROUND

1. Field of Invention

The present invention is generally directed to the field of roboticmanipulation of objects. More specifically, it is directed towardsrobotic recognition and manipulation of cable harnesses.

2. Description of Related Art

In the field of automated, or robotic, manufacturing or assembly, theability to identify assembly components, manipulate and attach them toother components is very important. Often, this is achieved by use ofassembly stations, where each assembly station is limited to onecomponent having one known orientation and requiring simplifiedmanipulation.

It would be advantageous, however, for a machine to be able to select aneeded component from a supply of multiple components, identify any keyassembly features of the component, and manipulate the selectedcomponent as needed for assembly. This would require that the machinehave some capacity for computer vision, object recognition andmanipulation.

Before discussing some details of computer vision, however, it isbeneficial to first discuss how computer vision has been used in thefield of robotic (or machine) vision. Two important aspects of roboticvision are the identifying of an object and the estimating of its pose,i.e., its 3-dimensional (i.e., 3D) orientation relative to a knownreference point and/or plane.

Since most cameras take 2-dimensional (i.e., 2D) images, many approachesattempt to identify objects in a 2D image and infer some 3D informationfrom the 2D image. For example, in “Class-specific grasping of 3Dobjects from a single 2D image”, by Chiu et al., The 2010 IEEE/RSJInternational Conference on Intelligent Robots and Systems, Oct. 18-22,2010, Chiu et al. describe superimposing 2D panels in the form ofsimplified 2D shapes on the surface of objects in a 2D image. The 2Dpanels on each imaged object form a set defining the object in the 2Dimage. The generated 2D panels can then be compared with a library ofpanel sets that define different types of predefined 3D objects, such asa car. Each library panel set is compared from different view directionswith the generated 2D panels of the imaged object in an effort to find arelatively close match. If a sufficiently match is found, then inaddition to having identified the object, one has the added benefit ofhaving a good guess as to its orientation given the matched orientationof the 2D panel set of the predefined 3D object in the library.

As a second example is found in “Human Tracking using 3D Surface ColourDistributions” by Roberts et al., Image and Vision Computing, 2006, byRoberts et al. In this example, Roberts et al describe a system wheresimplified 2D shapes are superimposed on known rigid parts human body(such as the head, torso, arms, etc) as shown in a 2D video image. Themovements of the superimposed, simplified 2D shapes follow the movementsof the moving human in the 2D video. By analyzing the movements of the2D shapes, it is possible to discern the movement of the imaged human.

As is stated above, however, identifying a desired object in an image isonly part of the solution, particularly when dealing with movingobjects. In such cases, one further needs to discern information aboutthe viewed object's pose, or orientation, and possible movement throughspace. Various approaches have been used to address this need.

For example, in “3D Pose Estimation for Planes”, by Xu et al., ComputerVision Workshops (ICCV Workshops), 2009 IEEE 12th InternationalConference on Sep. 27 2009-Oct. 4 2009. Xu et al. describe using a planeoutline on the surface of a target object in a non-stereo image, andestimating the plane's normal direction to estimate the object's poseorientation.

A second example is found in “Robust 3D Pose Estimation and Efficient 2DRegion-Based Segmentation from a 3D Shape Prior”, by Dambreville et al.,European Conference on Computer Vision ICCV, 2008. Dambreville et al.describe segmenting a rigid, known, target object in a 2D image, andestimating its 3D pose by fitting onto the segmented target object, thebest fitting 2D projection of known 3D poses of the known target object.

A third example is provided in “Spatio-temporal 3D Pose Estimation ofObjects in Stereo Images” by Barrois et al., Proceedings of the 6thinternational conference on Computer vision systems, ICVS'08. Barrois etal. describe using a 3D object's normal velocity (defined by theobject's main direction of movement) at one point in time to estimateits pose at another point in time along a movement path.

Returning to the subject of computer vision, it is generally desirablethat an image not only be captured, but that a computer be able toidentify and label various features within the captured image.Basically, a goal of computer vision is for the computer to duplicatethe abilities of human vision by electronically perceiving andunderstanding the contents of a captured image. This involves extractingsymbolic information from image data using models constructed with theaid of geometry, physics, statistics, and learning theory. Thus, thefield of computer vision includes methods for acquiring, processing,analyzing, and gleaning an understanding of imaged objects, in order toform decisions.

Various approaches to identifying features within a captured image areknown in the industry. Many early approaches centered on the concept ofidentifying shapes. For example, if a goal was to identify a specificitem, such as a wrench or a type of wrench, then a library of thedifferent types of acceptable wrenches (i.e., examples of “true”wrenches) would be created. The outline shapes of the true wrencheswould be stored, and a search for the acceptable shapes would beconducted on a captured image. Shapes within a captured image might beidentified by means of a segmentation process where the outline offoreground objects is differentiated from an image's background. Thisapproach of shape searching was successful when one had an exhaustivelibrary of acceptable shapes, the library was not overly large, thesubject of the captured images did not deviate from the predefined trueshapes, and the background surrounding the target object was not overlycomplicated.

For complex searches, however, this approach is not effective. Thelimitations of this approach become readily apparent when the subjectbeing sought within an image is not static, but is prone to change. Forexample, a human face has definite characteristics, and its distortionis limited, but it still does not have an easily definable number ofshapes and/or appearance it may adopt. It is to be understood that theterm appearance is herein used to refer to color and/or lightdifferences across an object, as well as other surface/texturevariances. Other objects may be prone to far deformation than a humanface. For example, cable harnesses have definite characteristics, butmay take many different shapes and arrangements due to their wiringlacking many, if any, rigid structure. Nonetheless, it is still helpfulto look at some of the computer vision approaches used in facerecognition, as some aspects in this field can be applied to computervision, in general.

Although an exhaustive library of samples of a known rigid body may becompiled for identification purposes, it is self-evident that compilingan exhaustive library of human faces, or any non-rigid or amorphousobject, and their many variations is a practical impossibility. Thus,statistical methods have been developed to address these difficulties.

Developments in image recognition of objects that change their shape andappearance, are discussed in “Statistical Models of Appearance forComputer Vision”, by T. F. Cootes and C. J. Taylor (hereinafter Cooteset al.), Imaging Science and Biomedical Engineering, University ofManchester, Manchester M13 9PT, U.K. email: t.cootes@man.ac.uk,http://www.isbe.man.ac.uk, Mar. 8, 2004, which is hereby incorporated inits entirety by reference.

As Cootes et al., explain, in order for a machine to be able tounderstand what it “sees”, it must make use of models that describe andlabel the expected structure being imaged. In the past, model-basedvision has been applied successfully to images of man-made, rigidobjects having limited and known variations. Model-based vision,however, has proven more difficult in interpreting images of non-rigidobject having unknown variations, such as images of natural subjects,which tend to be complex and variable. A problem is the variability ofthe subject being examined. To be useful, a model needs to be specific,that is, it should be limited to representing true examples of themodeled subject. The model, however, also needs to be general andflexible enough to represent other plausible example (i.e., otherpossible true example not specifically available in a sample library) ofthe class of object it represents. It has been shown that this apparentcontradiction can be handled by statistical models that can capturespecific patterns of variability in shape and appearance. It has furtherbeen shown that these statistical models can be used directly in imageinterpretation.

To facilitate the application of statistical models, subjects to beinterpreted are typically separated into classes (i.e., category ofobjects). This permits the statistical analysis to use prior knowledgeof the characteristics of a particular class of object to facilitate itsidentification and labeling, and even to overcome confusion caused bystructural complexity, noise, or missing data.

Additionally, in order to facilitate further processing of identifiedand labeled subjects within a captured image, it is beneficial for theidentified subject to be transformed into (i.e., be fitted onto) apredefined, “model” shape with predefined locations for labeled items.For example, although the human face may take many shapes and sizes, itcan be conformed to a standard shape and size. Once conformed to thestandard shape and size, the transformed face can then be furtherprocessed to determine its expression, determine its gaze direction,identify the individual to whom the face belongs, etc.

A method that uses this type of alignment is the active shape model.With reference to FIG. 1, the active shape model uses a predefined modelof a class of object, such as human face 1A in the present example, anda list of predefined deformation parameters, each having correspondingdeformation constraints, to permit the predefined model to be stretchedand move to attempt to align it with a subject image 2. Alternatively,the list of predefined deformation parameters may be applied to subjectimage 2, and have it be moved and deformed to attempt to align it withthe predefined model 1A. This alternate approach has the added benefitthat once subject image 2 has been aligned with the predefined model 1A,it will also be fitted to the shape and size of the predefined model 1A,which facilitates the identifying of individual parts of the subjectimage 2 in accordance with labels on the predefined model 1A.

For illustrative purposes, FIG. 1 shows predefined model (i.e., modelface) 1A being fitted to subject image (i.e., subject face) 2. Theexample of FIG. 1 is an exaggerated case for illustration purposes. Itis to be understood that a typical model face 1A would have constraintsregarding its permissible deformation points relative to other pointswithin itself. For example, if aligning the model face meant moving itsleft eye up one inch and moving its right eye down one inch, then theresultant aligned image would likely not be a human face, and thus sucha deformation would typically not be permissible. It is to beunderstood, however, that this limitation would not apply to non-rigidobject that can take large amounts of deformation, such as cableharnesses.

In the example of FIG. 1, the model face 1A is first placed roughlywithin the proximity of predefined points of interest, and typicallyplaced near the center of subject face 2, as illustrated in image 3. Bycomparing the amount of misalignment resulting from moving model face 1Ain one direction or another, and the results of adjusting a sizemultiplier in any of several predefined directions, one can determinehow to better align model face 1, as illustrated in image 4. Anobjective would be to align as closely as possible predefined landmarks,such as the pupils, nostril, mouth corners, etc., as illustrated inimage 5. Eventually, after a sufficient number of such landmark pointshave been aligned, the subject image 2 is warped onto model image 1Aresulting in a fitted image 6 with easily identifiable and labeledfeatures of interest that can be further processed to achieve specificobjectives.

This approach, however, does not take into account changes inappearance, i.e., shadow, color, or texture variations for example. Amore holistic, or global, approach that jointly considers the object'sshape and appearance is the Active Appearance Model (AAM). AlthoughCootes et al. appear to focus primarily on the gray-level (or shade)feature of appearance, they do describe a basic principle that AAMsearches for the best alignment of a model face (including both modelshape parameters and model appearance parameters) onto a subject facewhile simultaneously minimizing misalignments in shape and appearance.In other words, AAM applies knowledge of the expected shapes ofstructures, their spatial relationships, and their gray-level appearance(or more generally color value appearance, such as RGB values) torestrict an automated system to plausible interpretations. Ideally, AAMis able to generate realistic images of sought objects. An example wouldbe a model face capable of generating convincing images of anindividual, such as by changing the individual's expression and so on.AAM thus formulates interpretation as a matching problem: given an imageto interpret, structures are located and labeled by adjusting themodel's parameters in such a way that it generates an ‘imagined image’that is as similar as possible to the real thing.

Although AAM is a useful approach, implementation of AAM still posesseveral challenges. As stated above, an AAM machine generates resultsfrom the application of statistical analysis of a library of truesamples to define distinguishing parameters and the parameter'spermissible distortions. By the nature of the statistical analysis, theresults will permit alignment only with a fraction of all true samples.If the subject category is prone to a wide range of changes, such ascable harness that can take any distortion when dropped onto an assemblyline (such as a conveyor belt) or when picked up, the model may not beable to properly align itself to an input subject image withcharacteristics beyond the norm defined by the shape or appearancemodel.

Another limitation of an AAM machine is that construction of the model(or conical) image (i.e., model face 1A in the example of FIG. 1)requires much human intervention to identify the distinguishing featuresof the specific object being sought.

For example with reference to FIG. 2, model face 1A may be constructedfrom a library of training images 1 (i.e., true face images). Typically,a user manually places “landmark” points on each training image tooutline specific features characteristic to the class of object beingrepresented. The landmark points are ideally selected in such a way thatthe landmark points outline distinguishable features within the classcommon to every training image. For instance, a common feature within aface class may be the eyes, and when building a model of the appearanceof an eye in a face image, landmark points may be placed at the cornersof the eye since these features would be easy to identify in eachtraining image. In addition to the landmark points, however, an activeappearance model (AAM) machine also makes use of appearance data (i.e.,shade data and/or color data and/or texture data, etc.) at variouspatches of each training image to create a distribution range ofacceptable appearances for corresponding patches within model face 1A.This appearance data constitutes additional features in the overallstatistical analysis.

Thus, an AAM machine may be too complicated and computationallyintensive for practical machine vision applications in industrialassembly lines where the object class is prone to great deformation,such as when the object class is one or more types of wire harnesses.Thus, machine vision applications typically rely on more automatedmethods of identifying characteristic features and object edges in acaptured image. Additionally if a machine is expected to interact withan object in an assembly line, such as if a robot is intended to pick upa specific type of wire harness from a bin of multiple wire harnessesand attach (i.e., plug) a specific end of the harness to a specificreceptacle, the machine will need some sort of depth perception toproperly manipulate the robot.

It is further noted that edge detection algorithms are part of manyimage manipulation operations. Edge detection is fundamental to imageprocessing and computer vision, particularly in the areas of featuredetection and feature extraction. Edge detection aims to identifypoints, i.e., pixels that outline objects within an image. There aremany edge detection algorithms, but generally they attempt to identifypixels at which discontinuities occurs, i.e., where the image brightnesschanges sharply. In the ideal case, the result of applying an edgedetector to an image leads to a set of connected curves that indicatethe boundaries of objects, the boundaries of surface markings, anddiscontinuities in surface orientation. Once the boundaries have beenidentified, various image processing operations may be applied to thedigital image.

For example FIG. 3A shows a typical digital image, and FIG. 3B shows theresults of applying edge detection to the image of FIG. 3A. Edgedetection may be designed to identify thick or thin lines, or may beoptimized to separately identify thick and thin lines. In the example ofFIG. 3B, both thick and thin lines are separately identified, whichpermits them to be separately processed. This permits the processing ofthe digital image to be more specialized by adjusting the size of apixel-processing window according to line thickness. As a result,application of a specific image processing algorithms, such a bilateralfilter, may be optimized along the edge of objects according to linethickness to achieve a sharper final image, as shown in FIG. 3C.

Another use of edge detection is feature detection. As an example, ifone has a library of identifying features of a specific object, then onemay search an input digital image for those identifying features in aneffort to determine if an example of the specific object is present inthe input digital image. When this is extended to multiple digitalimages of a common scene taken from different view angles, it ispossible to index, i.e., match or correlate, feature points from oneimage to the other. This permits the combined processing of the multipledigital images.

For example in FIG. 4, images 7A, 7B, 7C and 7D each provide partial,and overlapping, views of a building in a real-world scene, but noneprovide a full view of the entire building. However, by applying edgedetection and indexing (i.e., identifying matching pairs of) featurepoints in the four partial images 7A, 7B, 7C and 7D that correlate tothe same real feature point in the real-world scene, it is possible tostitch together the four partial images (i.e., applying an imagestitching tool) to create one composite image 7E of the entire building.The four partial images 7A, 7B, 7C and 7D of FIG. 4 are taken from thesame view angle, but this approach may be extended to the field ofcorrespondence matching, where images of a common scene are taken fromdifferent view angles.

Images of a common scene are taken from different view angles are thebasis for stereo vision and depth perception. In this case,corresponding feature points in two images taken from different viewangles (and/or different fields of vision) of the same subject (orscene) can be combined to create a perspective view of the scene. Thus,imaging a scene from two different view points (i.e., from two differentfields of vision, FOV) creates stereo vision, which provides depthinformation about objects in the scene.

This ability would be particularly helpful in the field of robotics andautomated assembly/construction. In these applications, a machine havingstereo vision and the ability to discern (i.e., identify) target itemswould have the ability to independently retrieve the target item and useit in an assembly.

Implementing such vision capabilities, however, is still a challenge,even in a specialized assembly line where the number of possible targetobject variants is limited. The challenges become even more dauntingwhen the target objects are amorphous, or otherwise prone to change inshape and/or appearance, such as in the case of wire harnesses.

It is an object of the present invention to provide a system foridentifying and manipulating cable harnesses for use in robotic assemblylines.

It is a further object of the present invention to make use of 3Dinformation for determining pose information of cable harnesses.

It is a further object of the present invention to provide a 3D visualsystem suitable for use in a robotic assembly line.

SUMMARY OF INVENTION

The above objects are met in a cable harness visualization system usingstereo imaging to view cable harnesses, determine their pose, andidentify specific segments (and/or parts) of the viewed cable harnessfor manipulation.

A 2D cable localization module identifies pairs of commonly imagedcables in a pair of stereo images. A 3D cable localization module thencreates a 3D point cloud of the identified pairs of commonly imagedcables. Alternatively, a 3D imaging system may create a 3D model of animage harness, which includes construction of a 3D point cloudrepresentation of the physical cable harness. The 3D imaging system maybe include, for example, a 3D laser scanner, a KINECT sensor (such asproduced by MICROSOFT Corp.), and/or a time-of-flight device or similardevice, such as a range camera. Irrespective of how the 3D point cloudis constructed, normal directions are determined for points within the3D point cloud to determine the image cable's pose.

A cable structure representation module then redefines the complex, 3Dpoint clouds in terms of simplified, predefined, 3D primitive shapes.The shapes may be cylinders or parallel prisms for the non-rigid parts(i.e., the wires) of a cable harness. Cable connectors may also bedefined by 3D geometric primitive shapes, or they may be defined be 3Dprimitive shapes determined from CAD (computer-aided design) files thatdefine the connector. Each non-rigid cable segment is defined by trainsof 3D primitive shapes of known (and stored) length(s). The lengths maybe determined on-the-fly on a case-by-case basis, or may bepredetermined. In this manner, a particular point on the non-rigid cablesegment may be determined by counting the number of primitive shapesneeded to reach the particular point along the cable harness.

The above objects are further met in a cable harness visualizationsystem, comprising: a three-dimensional, i.e., 3D, imaging systemimaging at least one physical cable harness and creating a 3D cableharness model of the imaged physical cable harness; a cable structurerepresentation module representing the 3D cable harness model in termsof sets of predefined primitive 3D shapes and node markers, eachprimitive 3D geometric shape being of known dimensions, the imagedphysical cable harness having connectors and non-rigid cable segments,the node markers defining the opposing ends of the non-rigid cablesegments, each non-rigid cable segment being joined to a connector or toanother non-rigid cable segment at a node marker; wherein: eachnon-rigid cable segment is represented by a corresponding one of thesets of predefined primitive 3D shapes; the corresponding set ofpredefined primitive 3D shapes consisting of a train of primitive 3Dgeometric shapes that spatially track the path of its correspondingnon-rigid cable segment, each non-rigid cable segment extends into theinterior of its corresponding train of geometric shapes, and theindividual geometric shapes within a train of geometric shapes turn inaccord with twists and turns of the non-rigid cable segment theyrepresent.

Preferably, in the cable structure representation module, IF a non-rigidcable segment consists of a plurality of wires substantially coupledlinearly side-by-side each other to a connector, THEN its correspondingtrain of primitive 3D geometric shapes consists of a train ofrectangular prisms; ELSE its corresponding train of primitive 3Dgeometric shapes consists of a train of cylinders.

Additionally, the widths of the rectangular prisms may substantiallyspan across the side-by-side wires they represent, and the diameters ofthe cylinders substantially encompass the portion of the non-rigid cablesegment they represent.

Furthermore, all rectangular prisms within the train of rectangularprisms may have a common first length, and all cylinders within train ofcylinders may have a common second length.

If desired, each non-rigid cable segment may is characterized by a lackof joints or rigid portions between its opposing ends.

Additionally, the 3D imaging system may include at least one of atime-of-flight device, a 3D laser scanner, a KINECT sensor, and a rangecamera.

Preferably, the 3D cable harness model is based on a point cloud.

Further preferably, the 3D imaging system includes: a stereo imagingdevice producing stereoscopic image pairs of the physical cableharnesses, each stereoscopic image pair including a first image and asecond image; a two-dimensional, i.e., 2D, cable localization modulelocalizing commonly imaged physical cable harnesses in the first andsecond images, the 2D cable localization module further identifyingcorresponding pixel pairs in the first and second images, eachcorresponding pixel pair including a first pixel from the first imageand a second pixel from the second image, both first and second pixelscorresponding to a commonly imaged point on a commonly imaged physicalcable harness; a 3D cable localization module creating a 3D point cloudrepresentation of each commonly imaged physical cable harness in 3Dspace in accordance with perspective constraints and the commonly imagedphysical cable harness' corresponding pixel pairs; a 3D pose estimatordetermining cable pose orientations of commonly imaged physical cableharnesses in the 3D space in accordance with their corresponding 3Dpoint clouds, the 3D pose estimator determining a surface normaldirection for selected points within the 3D point clouds relative to aneighborhood-of-points of predefined size surrounding each selectedpoint, the 3D cable harness model being defined by a corresponding 3Dpoint cloud and corresponding cable pose orientations.

In this case, the 3D pose estimator determines the surface normaldirection for all points within all 3D point clouds.

Additionally, the 2D cable localization module segments the first andsecond image to define image segments, and each image segment defines asilhouette of an imaged cable harness.

Following this approach, it is preferred that each image segment bedefined by: (I) for each unsegmented part of an image, selecting a seedpoint within the unselected part, and for each selected pointiteratively applying the following steps: (i) determine similaritymeasures for the color similarities between the seed point and itsnearest neighbor pixels, the nearest neighbor pixels being candidatepixels; (ii) join the seed point and the candidate pixels if thesimilarity measures are higher than a predefined threshold, and IF anyof the joined pixels are a part of an existing image segment, THEN theexisting image segment is grown to include the joined pixels, ELSE thejoined pixels define a new image segment; (iii) determine a region colordistribution for the image segment of step (ii) and calculate itsprinciple component; (II) returning to step (I) until all points withinan image have been selected, in turn.

Further preferably, the system includes after step (II): step (III) ofjoining together any proximate image segments having a region colordistribution similarity within a predefined first threshold and ageometric properties similarity within a predefined second threshold.

In this case, proximate image segments may be defined as image segmentsseparated by not more than 60 pixels.

It is further preferred that the region color distribution similarityand the geometric properties similarity between image segments bedetermined from their respective principle components, within predefinedthresholds.

The preferred system further includes including after step (II): (a)determining a first set of feature descriptors for the image segments inthe first image and a second set of feature descriptors for imagesegments in the second image; and (b) defining the corresponding pixelpairs by matching pairs of corresponding feature descriptors between thefirst and second sets of feature descriptors.

In this case, it is preferred that step (b) further include: identifyingas a candidate matching descriptor, a feature descriptor in the secondset that matches a given feature descriptor in the first set; and IF itsrelative position within the second image differs from the relativeposition of the given feature descriptor in the first image by more thana predefined margin, THEN discarding the candidate matching descriptor,ELSE deeming the candidate matching descriptor as a corresponding pixelpair to the given feature descriptor.

It is also preferred that step (b) feature descriptors in the first andsecond sets of feature descriptors be matched by means of a tree-basedfeature matching scheme.

It further envisioned that the 3D pose estimator implement the followingsteps: (a) applying global homography to the first and second images toreject corresponding pixel pairs in the first and second images that donot satisfy global homography constraints; and (b) for eachcorresponding pixel pair not rejected in step (a), applying localhomography in accordance to its corresponding image segments to furtherremove from each image segment any points that meeting local homographyconstraints.

Additionally in the cable harness visualization system, the 3D poseestimator may determine a surface normal direction for a selected pointby; defining a local window around the selected point; identifying the3D points within the defined local window; fitting a 2D plane on to theidentified 3D points, and estimating the local 3D normal direction ofthe fitted 2D plane.

The above objects are further met in a robotic system for manipulatingcable harnesses, where the robotic system implements any of the cableharness visualization system described above.

Other objects and attainments together with a fuller understanding ofthe invention will become apparent and appreciated by referring to thefollowing description and claims taken in conjunction with theaccompanying drawings.

BRIEF DESCRIPTION OF THE DRAWINGS

In the drawings wherein like reference symbols refer to like parts.

FIG. 1 illustrates the fitting of an object within an image to a modelshape of that object class.

FIG. 2 illustrates the defining of a model shape of a specific objectclass, i.e., a human face, by combining characteristic of a collectionof training images and provided “landmark” points.

FIGS. 3A, 3B, and 3C illustrate the use of edge detection for thepurposes of selecting filter types for specific parts of an image toimprove image clarity.

FIG. 4 illustrates the use of corresponding feature points in differentimages to stitch together the image to create a larger composite image.

FIG. 5 illustrates the principles of Epipolar geometry.

FIG. 6 is an example of defining stereo constraints using Epipolargeometry.

FIG. 7 illustrates the establishment of homography constraints fromstereo constraints.

FIG. 8 illustrates homography to re-establish a perspective (i.e., 3D)view from a stereo pair of image taken of a common scene.

FIG. 9 illustrates feature point extraction from a sample image.

FIG. 10 illustrates the establishment of feature point correspondenceusing an SIFT transform.

FIG. 11 illustrates the establishment of feature point correspondenceusing an ASIFT transform.

FIG. 12 is an example of feature point correspondence in two images of acommon scene, taken from different a field-of-visions, i.e., FOV.

FIG. 13 illustrates that each feature point is defined by a128-dimension vector comprised of multiple histograms of image pixelcharacteristics to attempt to uniquely define a pixel, i.e., a featurepoint.

FIGS. 14, 15, and 16 illustrate one method of arranging the informationof extracted feature points into a hierarchical tree to ease comparisonof feature points betweens images.

FIG. 17 is an overflow of a preferred embodiment of the presentinvention.

FIGS. 18A, 18B and 18C illustrate the operation of rectification block105 of FIG. 17.

FIG. 19 illustrates the operation of 2D localization block 107 of FIG.17.

FIGS. 20A, 20B and 20C illustrate various processing steps of 2Dlocalization block 107.

FIGS. 21A and 21B are a second example of the processing of 2Dlocalization block 107.

FIG. 22 illustrates the operation of stereo matching block 109 of FIG.17.

FIGS. 23A and 23B illustrate a triangulation operation as implemented bystereo matching block 109 of FIG. 17.

FIG. 24 illustrates a reconstructed 3D cable geometry shown in twodifferent views.

FIG. 25 illustrates the estimating of surface normals at points on acable, as implemented by 3D localization block 111 of FIG. 17.

FIG. 26 illustrates the defining of a cable harness into in terms ofcable units, nodes and connectors.

FIG. 27 is an example of round cable units represented by cylinders, asthe primitive geometric shape.

FIG. 28 is an example of flat cable units represented by rectangularprisms, as the primitive geometric shape.

FIG. 29 illustrates a sequence of estimated 3D cable primitives(cylinders) using the 3D cable point clouds and surface normals (twoviews).

FIG. 30 illustrates geometric model fitting to estimate cableprimitives.

FIG. 31 illustrates the function of primitive shape fitting block 119 ofFIG. 17.

FIG. 32 illustrates a robot utilizing the present invention tomanipulate a cable harness.

DESCRIPTION OF THE PREFERRED EMBODIMENTS

Automated assembly of components is critical to the manufacture of manyitems. Often, automated assembly is limited to relatively rigidcomponents, or sub-components to facilitate the manipulation of thecomponents. There is a need, however, for a robotic assembly to be ableto manipulate non-rigid bodies. In particular, it would be beneficialfor an automated system to be able to manipulate cable harness, whichhave a non-rigid structure and are prone to take many differentconfigurations. It is further beneficial for such an automated system tobe able to distinguish between different cable harnesses in acollection, and to be further able to identify specific parts of thecable harness. There is a further need for such an assembly to be ableto discern the pose, i.e., orientation or arrangement, of cableharnesses in order identify specific connectors on the cable harnesses,and properly align a connector to a specific receptacle of theconnector.

It is presently preferred that such an automated system be able toreceive 3-dimensional (3D), i.e., perspective, images, which providedepth information about a scene, such as an assembly line, and extractpose information about an imaged cable harness from the 3D image.

The 3D images may be produced by means of a 3D imaging system, such as a3D laser scanner, a MICROSOFT CORP. KINECT sensor, a range camera, orany of many different types of time-of-flight devices. Preferably,however, the 3D images are produced using a stereo imaging system, whichextracts 3D information from a pair of stereoscopic images (i.e., astereoscopic image pair). As it is known in the art, each respectiveimage in a stereoscopic image pair is produced by a respective one oftwo 2D imaging cameras spaced apart to provide two views of a commonscene.

Irrespective of the 3D imaging technique used, it is desirable that the3D imaging technique produce a point cloud of the imaged 3D object. Asit is known in the art, a point cloud is a collection of points spreadalong he surface of a 3D object.

Before proceeding with a discussion of how to determine pose informationfrom a 3D image (or 3D image model of a cable harness), it is beneficialto first provide general discussion of how 3D information (i.e.,perspective information) may be extracted from a stereoscopic imagepair.

In order to extract 3D information from a stereoscopic image pair, onefirst needs to be able to identify commonly imaged items in thestereoscopic image pair. However, since both images of the stereoscopicimage pair provide different views of the commonly image items, this canbe a difficult task. One needs to recognize different views of the sameobject in 2D images, and to correlate specific parts of the commonlyimaged object.

Object recognition (or object identification) is an integral part ofcomputer vision, and an integral part of object recognition is patternmatching. An essential component of pattern matching in images (andparticularly in 2D images) is feature detection, which refers toidentifying parts of an image, or individual feature points of an image(such as individual pixels), that are good candidates for investigationto determine if they might be part of a sought after object in an image.

Various techniques are known for identifying feature points, orindividual pixels, in an image that may be used to describe an imagedscene. As an example, if one has a library of identifying feature pointsobtained from a library of training images, then one may search an inputdigital (test) image for those identifying features in an effort todetermine if an example of the specific object is present in the inputdigital image. In the field of computer vision, this idea has beenextended to matching common features of a common scene in multipledigital images of the common scene taken from different view angles toindex, i.e., match or correlate, feature points from one image to theother. This permits the combined processing of the multiple digitalimages.

For example in FIG. 4, images 7A, 7B, 7C and 7D each provide partial,and overlapping, views of a building in a real-world scene, but noneprovide a full view of the entire building. However, by applying edgedetection and indexing (i.e., identifying matching pairs of) featurepoints in the four partial images 7A, 7B, 7C and 7D that correlate tothe same real feature point in the real-world scene, it is possible tostitch together the four partial images (i.e., applying an imagestitching tool) to create one composite image 7E of the entire building.The four partial images 7A, 7B, 7C and 7D of FIG. 4 are taken from thesame view angle, but this approach may be extended to the field ofcorrespondence matching.

Correspondence matching refers to the matching of objects or objectfeatures (or more typically, the matching of feature points, i.e.,individual pixels) common to two or more images. Correspondence matchingtries to figure out which parts of a first image correspond to (i.e.,are matched to) which parts of a second image, assuming that the secondimage was taken after the camera that took the first image had moved,time had elapsed, and/or the pictured objects had moved. For example,the first image may be of a real-world scene taken from a first viewangle with a first field-of-vision, FOV, and the second image may be ofthe same scene taken from a second view angle with a second FOV.Assuming that the first and second FOVs at least partially overlap,correspondence matching refers to the matching of common features pointsin the overlapped portions of the first and second images.

Thus, correspondence matching is an essential problem in computervision, especially in stereo vision, view synthesis, and 3D (orperspective) reconstruction. Assuming that a number of image features,or objects, in two images taken from two view angles have been matched,epipolar geometry may then be used to identify the positionalrelationship between the matched image features to achieve stereo viewsynthesis, or 3D reconstruction.

Epipolar geometry is basically the geometry of stereo vision. Forexample in FIG. 5, two cameras 11 and 13 create two 2D images 15 and 17,respectively, of a common 3D scene 10 consisting of a larger sphere 19and a smaller sphere 21. 2D images 15 and 17 are taken from two distinctview angles 23 and 25. Epipolar geometry describes the geometricrelations between points in 3D scene 10 (for example spheres 19 and 21)and their relative projections in 2D images 15 and 17. These geometricrelationships lead to constraints between the image points, which arethe basis for epipolar constraints, or stereo constraints, describedmore fully below.

FIG. 5 illustrates a horizontal parallax where, from the view point ofcamera 11, smaller sphere 21 appears to be in front of larger sphere 19(as shown in 2D image 15), but from the view point of camera 13, smallersphere 21 appears to be some distance to the side of larger sphere 19(as shown in 2D image 17). Nonetheless, since both 2D images 15 and 17are of the same 3D scene 10, both are truthful representations of therelative positions of larger sphere 19 and smaller sphere 21. Thepositional relationships between camera 11, camera 13, smaller sphere 21and larger sphere 19 thus establish geometric constraints on 2D images15 and 17 that permit one to reconstruct 3D scene 10 given only 2Dimages 15 and 17, as long as the epipolar constraints (i.e., stereoconstraints) are known.

Epipolar geometry is based on the pinhole camera model, a simplifiedrepresentation of which is shown in FIG. 6. In the pinhole camera model,cameras are represented by a point, such as left point O_(L) and rightpoint O_(R), at each respective camera's focal point. Point P_(O)represents the point of interest (i.e., an object) in the 3D scene beingimaged, which in the present example is represented by two crisscrossedlines.

Typically, the image plane (i.e., the plane on which a 2D representationof the imaged 3D scene is captured) is behind a camera's focal point andis inverted. For ease of explanation, and to avoid the complications ofa an inverted captured image, two virtual image planes, ImgL and ImgR,are shown in front of their respective focal points, O_(L) and O_(R), toillustrate non-inverted representations of captured images. One maythink of these virtual image planes as windows through which the 3Dscene is being viewed. Point P_(L) is the 2D projection of point P_(O)onto left virtual image ImgL, and point P_(R) is the 2D projection ofpoint P_(O) onto right virtual image ImgR. This conversion from 3D to 2Dmay be termed a perspective projection, or image projection, and isdescribed by the pinhole camera model, as it is known in the art. It iscommon to model this projection operation by rays that emanate from acamera and pass through its focal point. Each modeled emanating raywould correspond to a single point in the captured image. In the presentexample, these emanating rays are indicated by dotted lines 27 and 29.

Epipolar geometry also defines the constraints relating the positions ofeach camera relative to each other. This may be done by means of therelative positions of focal points O_(L) and O_(R). The focal point of afirst camera would project onto a distinct point on the image plane of asecond camera, and vise-versa. In the present example, focal point O_(R)projects onto image point E_(L) on virtual image plane ImgL, and focalpoint O_(L) projects onto image point E_(R) on virtual image plane ImgR.Image points E_(L) and E_(R) are termed epipoles, or epipole points. Theepipoles and the focal points they project from lie on a single line,i.e., line 31.

Line 27, from focal O_(L) to point P_(O), is seen as a single pointP_(L) in virtual image plane ImgL, because point P_(O) is directly infront of focal point O_(L). This is similar to how in image 15 of FIG.5; smaller sphere 21 appears to be in front of larger sphere 19.However, from focal point O_(R), the same line 27 from O_(L) to pointP_(O) is seen a displacement line 33 from image point E_(R) to pointP_(R). This is similar to how in image 17 of FIG. 5; smaller sphere 21appears to be displaced to the side of larger sphere 19. Thisdisplacement line 33 may be termed an epipolar line. Conversely fromfocal point O_(R), line 29 is seen as a single point P_(R) in virtualimage plane ImgR, but from focal point O_(L) line 29 is seen asdisplacement line, or epipolar line, 35 on virtual image plane ImgL.

Epipolar geometry thus forms the basis for triangulation. For example,assuming that the relative translation and rotation of cameras O_(R) andO_(L) are known, if projection point P_(L) on left virtual image planeImgL is known, then the epipolar line 33 on the right virtual imageplane ImgR is known by epipolar geometry. Furthermore, point P_(O) mustprojects onto the right virtual image plane ImgR at a point P_(R) thatlies on this specific epipolar line, 33. Essentially, for each pointobserved in one image plane, the same point must be observed in anotherimage plane on a known epipolar line. This provides an epipolarconstraint that corresponding image points on different image planesmust satisfy.

Another epipolar constraint may be defined as follows. If projectionpoints P_(L) and P_(R) are known, their corresponding projection lines27 and 29 are also known. Furthermore, if projection points P_(L) andP_(R) correspond to the same 3D point P_(O), then their projection lines27 and 29 must intersect precisely at 3D point P_(O). This means thatthe three dimensional position of 3D point P_(O) can be calculated fromthe 2D coordinates of the two projection points P_(L) and P_(R). Thisprocess is called triangulation.

Epipolar geometry also forms the basis for homography, i.e., projectivetransformation. Homography describes what happens to the perceivedpositions of observed objects when the point of view of the observerchanges. An example of this is illustrated in FIG. 7, where the shape ofa square 12 is shown distorted in two image projections 14 and 16 asviewed from two different points of view V1 and V2, respectively. Likebefore, image planes 14 and 16 may be thought of as windows throughwhich the square 12 is viewed.

Homography would identify the points in common between image projections14 and 16 and square 12 (i.e., point registration). For example, thefour corners A, B, C and D of square 12 correspond to points A′, B′, C′and D′ in image projection 14, and correspond to points A″, B″, C″ andD″ in image projection 16. Thus, points A′, B′, C′ and D′ in imageprojection 14 correspond respectively to points A″, B″, C″ and D″ inimage projection 16.

Assuming that the pinhole model applies, epipolar geometry permitshomography to relate any two images of the same planar surface in space,which permits image rectification, image registration, or computation ofcamera motion (rotation and translation) between two images. Once camerarotation and translation have been extracted from an estimatedhomography matrix, this information may be used for navigation, or toinsert models of 3D objects into an image or video, so that they arerendered with the correct perspective and appear to have been part ofthe original scene.

For example in FIG. 8, cameras 22 and 24 each take a picture of a 3Dscene of a cube 26 from different points of view. From the view point ofcamera 22, cube 26 looks as shown in 2D image 28, and from the viewpoint of camera 24, cube 26 looks as shown in 2D image 30. Homographypermits one to identify correlating points, some of which are shown bydotted lines for illustration purposes. This permits both 2D images 28and 30 to be stitched together to create a 3D image, as shown in image32. Thus, automatically finding correspondence between pairs of imagesis the classic problem of stereo vision. Integral to this, however, isthe identifying of feature points in the pairs of images, and thematching of corresponding feature points in the pairs of images.

The above discussion of stereo vision, such as epipolar geometry andhomography, may be collectively referred to as perspective constraints,particularly as applied to a stereo image pair.

Because of their use in establishing perspective (i.e., 3D) information,feature based correspondence matching algorithms have found wideapplication in computer vision. Examples of feature based correspondencematching algorithms are the scale-invariant feature transform, SIFT, andthe Affine SIFT (or ASIFT). It is noted, however, that feature basedcorrespondence matching algorithms such as SIFT and Affine SIFTpurposely exclude edge points from their analysis, and thus are not wellsuited for edge detection.

As it is known in the art, the SIFT algorithm scans an image andidentifies points of interest, or feature points, which may beindividual pixels and describes them sufficiently (typically relative toits neighboring pixels within a surrounding window) so that the samefeature point (or pixel) may be individually identified in anotherimage. A discussion of the SIFT transform is provided in U.S. Pat. No.6,711,293 to Lowe, which is herein incorporated in its entirety byreference. Essentially, SIFT uses a library of training images toidentify feature points that are characteristic of a specific object.Once a library of the object's characteristic feature points have beenidentified, the feature points can be used to determine if an instanceof the object is found in a newly received test image. Other examples offeature point extraction are provided in “ORB: an efficient alternativeto SIFT or SURF” by Rublee et al., International Conference on ComputerVision, 2011.

Principally, feature points (i.e., points of interest) of the object areextracted to provide a “feature description” of a specific object. Thisdescription, extracted from training images, can then be used toidentify the specific object in a test image containing manyobject-types. To perform reliable recognition, it is preferred that thefeatures extracted from the training images be detectable under changesin image scale, noise, illumination, and rotation. Feature pointsusually lie near high-contrast regions of an image. However, sincedistortion of an object (such as if a feature points is located in anarticulated or flexible parts of the object) may alter a feature point'sdescription relative to its neighboring pixels, changes to an object'sinternal geometry may introduce errors. To compensate for these errors,SIFT typically detects and uses a large number of feature points so thatthe effects of errors contributed by these local variations may bereduced.

In a typical SIFT application, feature points of objects are firstextracted from a set of training images and stored in a database. Anobject is recognized in a new image (i.e., a test image) by individuallycomparing each feature point extracted from the new image with thefeature points in this database and finding candidate matching featuresbased on Euclidean distance of their feature point vectors. From thefull set of matches, subsets of feature points that agree on the objectand its location, scale, and orientation in the new image are identifiedto filter out good matches. Consistent clusters of good matches are thenidentified. Typically, each cluster of three or more features that agreeon an object and its pose is then subject to further detailed modelverification and subsequently outliers are discarded. Finally theprobability that a particular set of features indicates the presence ofa specific object is computed, given the accuracy of fit and number ofprobable false matches. Object matches that pass all these tests can beidentified as correct.

An example of a SIFT determination of feature points in an image isillustrated in FIG. 9. Possible feature points are first identified, asindicated by dark dots in image 8A. Possible feature points that have alow contrast are then discarded, as illustrate in image 8B. Finally,possible features points located on edges are removed, which leaves thefinal set of feature points shown in image 8C.

Thus, SIFT permits one to match feature points of an identified objectfrom one image to another. This is illustrated in FIG. 10, where threeimages of the same object, i.e., a happy face, are shown. Forillustration purposes, only four feature points, corresponding to pointsnear the eyes and the corners of the mouth, are shown. As indicated inFIG. 10, SIFT can match feature points from a first face 36 to a secondface 37 irrespective of a change in scale. SIFT can also match featurepoints from first face 36 to a third face 38 irrespective of rotation.However, SIFT has been found to have limited immunity to affinetransforms of images. That is, SIFT is limited to the amount of changein the view-angle an imaged object can undergo and still be identified.

A method of extending a SIFT transform to better handle affinetransformations is described in “ASIFT: A New Framework for Fully AffineInvariant Image Comparison” by Morel et al, SIAM Journal on ImagingSciences, vol. 2, issue 2, 2009, which is herein incorporated in itsentirety by reference.

With reference to FIG. 11, an Affine SIFT would be better able to matchfeature points from first face 36, to representations of the same objectthat have undergone affine transformations, as illustrated by happyfaces 39, 40, and 41.

An example of an application of an Affine SIFT transform is illustratedin FIG. 12, where multiple feature points are matched from a first image9A of the stature of liberty from a first view angle, to a second image9B of the statue of liberty from a different view angle and at adifferent scale.

A quick overview of the feature point extraction function of a SIFTfilter/algorithm/module/processor is illustrated in FIG. 13. Eachextracted feature point (such as those illustrated in FIG. 9-12) isdescribed by a series of metrics falling to several categories, i.e.,distinguishing characteristics. The metrics in each category constitutea histogram. Consequently, a typical SIFT processing algorithm creates aseries, or set, of SIFT histograms 65, and each set of histogramscollectively describes an individual item descriptor (or feature pointor SIFT descriptor). Each of SIFT histograms 65 statistically describesa distinguishing characteristic of the item descriptor relative toneighborhood of pixels (or pixel window) surrounding the item descriptorin the image being processed. The series of SIFT histograms 65 are thencollected into single vector 67, which defines one item descriptor. Thatis, each vector 67 provides sufficient data to identifying an individualpixel within an image. Therefore, each vector 67 describes a single itemdescriptor (i.e., a feature point or characteristic feature or (feature)pixel) and consists of 128 pieces of descriptive data. Thus, each itemdescriptor is characterized (i.e., described or identified) by a128-dimensioned vector 67.

The extracted feature points may then be matched to feature pointextracted from other images, and/or they may be used as a training basisto search for other instances of an object in other images. In thiscase, the extracted feature points are used as training feature point,and typically arranged in a searchable format, such as a hierarchicaltree.

For example, the item descriptors may be labeled to identify trainingsample images from which they were extracted. For example, a first group68 of item descriptors may be extracted from an image ID1_1. Similarly,a second group (or set) 70 of item descriptors may have been extractedfrom another image IDb_Bb. The SIFT descriptors corresponding to anygiven sample image constitutes a set of item descriptors for that imagethat may be used as a training set to train the SIFT for search anotherimage for an occurrence of the object pictured in the given sampleimage. For example first image ID1_1 is shown to have a set of Z itemdescriptors.

In one embodiment, all the sets of items descriptors from sample imagesof an object to be sought are collected into a composite collection ofitem descriptors, which is then used to construct a hierarchical tree.One method of achieving this is through a recursive k-means application,as is illustrated in FIGS. 14-16.

With reference to FIG. 14, although each item descriptor, such as point71, is a 128-dimension vector, for ease of illustration a clustering oflower-dimensioned item descriptors under a single center (preferably themean value) is shown. This mean value point 73 may define a root node 75of a hierarchical tree that may be constructed from the clustering offeature descriptors.

As is illustrated in FIG. 15, the item descriptor data is then splitinto two groups (for example two substantially equal groups) along meanpoint 73, as illustrated by dividing line 81. This creates two newcenter points 83 and 85 in the two newly created groups, respectively.As before, the two new center points 83 and 85 may be defined by themean of their respective groups of data. Each of center points 83 and 85may define respective child-nodes 77 and 79 under root node 75.

With reference to FIG. 16, each of these two groups may then be dividedalong their respective center points 83 and 85, as illustrated bydividing lines 87 and 89, respectively. This results in four newlycreated groups of data, each of which defines a new respective centerpoint 91, 93, 95 and 97. As before, center points 91, 93, 95 and 97 maybe defined by the mean of their respective group of data. Center points91 and 93 may define child-nodes 72 and 74 under node 77 in hierarchicaltree 45, and center points 95 and 97 may define child-nodes 76 and 78under node 79 in hierarchical tree 45. It is to be understood that thedata may continue to be divided to define additional child-nodes insimplified hierarchical tree 45. For example, each group of data maycontinue to be divided until the distance (i.e., the difference) betweendata within a group is not greater than a predefined maximum.

In a hierarchical tree structure, as it is known in the art, the rootnode is the top-most node in the hierarchical tree, a parent node is anode that has at least one other node below it and linked to it, a childnode is a node linked to a parent node above it, and a leaf node is anode with no child nodes below it. A leaf node is effectively abottom-most node along a link path (or branch path) downward from theroot node. A node along a path downward from the root node to a leafnode may be termed a “path node” or an “intermediate node”. Thus, in theexample of simplified hierarchal tree 45, node 75 is the root node,nodes 77 and 79 are intermediate nodes (i.e., nodes linked to a parentnode above them and linked to a child node below them), and nodes 72,74, 76 and 68 are leaf nodes (i.e., nodes linked to a parent node abovethem, but with no child nodes below them).

When determining if an instance of a sought object may be found in aninput image, feature points are extracted from the input (i.e., test)image in a similar manner as described above. These extracted featurepoints may be termed test feature points. The extracted test featurepoints may then be compared with sample feature points (i.e., trainingfeature points) extracted from sample images (i.e., training images) ofthe sought object or a specific image selected for comparison. Oneexample of how this may be done is if the extracted training featurepoints are arranged in a hierarchical tree structure as described above,and the extracted test feature pointes are then distributed into theexisting hierarchical tree structure. By observing the distribution, orthe clustering, of test feature points within hierarchical tree, one maydiscern if an instance of the sought object is indeed present. Thismight be done, for example, be measuring the correlation between thetest feature points and the training feature points within thehierarchical tree, and/or by a voting method, as is known in the art.

We now return the main portion of the present invention, which is thesensing (or determination) of pose information from a 3D model of acable harness, such as produced by a 3D imaging system. This may begenerally termed 3D sensing.

With reference to FIG. 17, the present system uses a 3D imaging system101 to generate a 3D model (which includes a 3D point cloud 113) of oneor more imaged cable harnesses (or other imaged subject, preferablyhaving a non-rigid body, or a partially non-rigid body). Preferably, 3Dimaging system 101 produces a point cloud 113 of the imaged cableharness. Thus, 3D imaging system 101 is the 3D sensing component of thepresent system.

3D sensing is crucial for robotic arms to effectively grasp andmanipulate cables on worktable, and may be implemented using 3D depthsensors such as a range scanner, time-of-flight device, 3D laserscanner, KINECT sensor from MICROSOFT CORP., and/or a range camera.

The presently preferred system, however, implements 3D imaging system101 using stereo image pairs. That is, the preferred embodiment uses astereo vision system for 2D localization, 3D reconstruction, and 3D poseestimation of cable harnesses. Further preferably, the system uses astereo-rig of two webcams (i.e., a stereo pair of cameras) that aresynchronized to capture images of cable harnesses (i.e., harnesscables).

The system first calibrates the stereo pair of cameras, as indicated bycalibration block 103, and rectifies camera images produced by thestereo pair of cameras, as indicated by rectification block 105.

2D localization block 107 then segments the images to identify 2Dlocations of individual pieces of the cable harness(es). These segmentsmay be defined by means of an connected component (CC) operation and/orsegmentation and/or other suitable operation known in the art. The 2Dlocalization block 107 further preferably recognizes that the identifiedsegments are only pieces (or parts) of a whole cable (i.e., cableharness), and selectively groups the pieces of each cable to form acomposite whole cable harness. This may be achieved by grouping piecesbased on consistency of appearance and geometrical properties. Forexample, pieces that are close to each other and have a similarappearance (i.e., color and/or shade and/or intensity) may be groupedtogether. Similarly, two pieces that appear to extend from one anotherbased on a consistent geometry of some features, such as the angle oftheir sides, may be grouped together. The composite cable harness(es)thus identify(ies) the 2D location(s) of complete cables within each ofthe stereo image pair.

Stereo matching of the complete cables is then implemented by stereomatching block 109. That is, corresponding cables in each of the twostereo image pairs and their commonly imaged features are identified.This may be implemented by applying tree-based stereo matching tocalculate the correspondence between the cable points across the leftand right images produced by the stereo pair of cameras.

In 3D localization block 111, the corresponding pair of left-right imagepixels is then triangulated to determine the 3D position of theassociated cable point(s), such as is explained above. 3D localizationblock 111 may thus produce a point cloud. If 3D imaging system 101 isimplemented by some function that has not yet produced a 3D point cloud(i.e., if a 3D point cloud has not yet been defined by the precedingblocks), then a point cloud may be produced at this time, as indicatedby 3D point cloud 113.

3D pose estimation block 115 then estimates the 3D pose (i.e., locationand orientation) at each cable position (or at each point (or group ofpoints of predefined size) on the point cloud) by fitting a local planeusing the estimated 3D cable points (i.e., the point cloud) andcalculating its surface normal.

Configurable structure representation block 117 then defines aconfigurable representation for the sophisticated cable harnessstructures, and primitive shape fitting block 119 fits the reconstructedpoints of each cable into a continuous chain of 3D primitive shapes,such as cylinders and/or rectangular prisms. The estimated cableprimitives can be fitted to a structure representation model and thesystem can control a robotic arm and hand to manipulate the cables usingthe estimated information.

An exemplary implementation is described below. For illustrationpurposes, the following system was implemented using a pair of robotarms, each having a hand. Additionally, 2 webcams were used, with animage Resolution of 2592×1944 pixels, a camera distance (Baseline) of 20cm., a camera convergence angle of 30 degrees, a tilt angle of 10degrees with respect to a worktable, a distance to the worktable of 60cm., and a visible area on worktable of 55 cm×55 cm.

Calibration of the stereo pair of cameras, as illustrate by calibrationblock 103, may be implemented using standard camera calibration methods.For example, a standard check-board-based calibration algorithm may beused. In this case, the camera is calibrated by placing a checkerboardis at different positions and orientations and capturing the images. Allthe camera parameters including focal length and 3D pose can then becalibrated using these images. An example of camera calibration using achecker-board is illustrated in “A flexible new technique for cameracalibration”, IEEE Transactions on Pattern Analysis and MachineIntelligence, Vol. 22, No. 11, pages 1330-1334, 2000, by Z. Zhang, whichis herein incorporated in its entirety by reference.

Preferably, rectification block 105 implements homography-based imagerectification using the calibrated camera parameters. An example of theoperation of Rectification block 105 is illustrated in FIGS. 18A through18C.

FIG. 18A shows an initial, non-calibrated left image 121 as produced bya first of the stereo camera pair, and an initial, non-calibrated rightimage 123 as produced by the second of the stereo camera pair.Rectification refers to the aligning and orienting of the images so thattheir subjects are aligned to each other as much as possible.

FIG. 18B shows the left and right images, respectively labeled 121′ and123′, after partial rectification. Partially rectified left and rightimages 121′ and 123′ have a common orientation, but are not yet aligned.

After rectification, as illustrate in FIG. 18C, corresponding points(i.e., pixels) in the rectified left image 121″ and rectified rightimage 123″ are located on the same horizontal line, as illustrated byhorizontal line 125. As is shown in FIG. 17, rectification block 105passes the left and right rectified images, 121″ and 123″, to 2Dlocalization block 107.

With reference to FIG. 20A, a second example of a rectified left image125 and a rectified right image 127 are shown. As is illustrated in FIG.19, the rectified left image 125 and rectified right image 127 fromrectification block 105 are passed to 2D localization block 107.Preferably, 2D localization block 107 includes a local regionsegmentation block 107 a and a grouping local segments to localizationcable regions block 107 b.

Local region segmentation block 107 a preferably defines color-basedregion growing for image segmentation. This would include, for example,a first step of selecting seed points whenever there is a pixel that isunsegmented. Then, for each seed point, the following steps may beapplied iteratively. 1) Calculate the color similarity between thepixels in the current region and the nearest neighbor pixels; 2) Includecandidate pixels to grow the region if similarity measures are higherthan experimentally-set thresholds; and 3) Update the region colordistribution and calculate the new principal component.

Alternatively, local segments may be defined using the method describedin “Efficient Graph-Based Image Segmentation”, by P. Felzenszwalb etal., IJCV, 2004, which is hereby incorporated by reference in itsentirety.

This can result in multiple independent segments within a common cableharness, as is illustrated in locally segmented left image 125′ andlocally segmented right image 127′ of FIG. 20B. These locally segmentedimages are then passed to block 170 b, whose job is to join the multiplelocal segments within a common cable harness into a single (a fewlarger) segment(s) spanning all (or much) of the cable harness, as isillustrated by grouped segments left image 125″ and grouped segmentsright image 127″ of FIG. 20C. The local segments (i.e., the cablesegments) may be grouped into complete cables by the grouping togethernearby cable segments (i.e., within 60 pixels) with similar appearanceand consistent geometrical properties (e.g. orientation). Detailed stepsmay include: (Step 1) Identify the connecting endpoints of each cablesegment with respect to its neighboring segments by using morphologicaldilation; (Step 2) Define a local window around the endpoints over thecable segment and compute the principal components (PCA) of the pixelcolors and local shape orientation; (Step 3) Compute the colorsimilarity and shape orientation similarity between neighboring segmentsusing the PCA values computed in step (2); (Step 4) Group theneighboring segments if both similarity measure is higher than aexperimentally-set threshold.

The locations of the composite, grouped segments define the 2D positionof the cable harness, which is output from 2D localization block 107 tostereo matching block 109, as is shown in FIG. 17.

Another example of locally group segmentation of cables in a left image131′ and a right 133′ is shown in FIG. 21A. The results of grouping thelocal group segments to define entire cable harnesses, defined by largersegments, and preferably a single larger segment (such as a connectedcomponent segment), for left image 131″ and right image 133″ areillustrated in FIG. 21B.

With reference to FIG. 22, the 2D position (and composite segmentgroups) of the cable harness in the rectified left and right images ispassed from 2D location block 107 to stereo matching block 109, as wasalso shown in FIG. 17 above. Stereo matching block 109 serves twofunctions. First it matches corresponding point in the rectified leftand right image, as is illustrated by stereo matching of (feature)points block 109 a, and generates a 3D point cloud of the physical cableharnesses represented in the rectified left and right images, as isillustrated by triangulation block 109 b. This results in the 3Dposition of cable harnesses relative to a reference point (and/orrelative to each other). Since the 3D position of cable harnesses isdefined by the 3D position of each matched (i.e., corresponding) pointin the rectified left and right images, it also defines a 3D point cloudfor each cable harness. FIGS. 23A and 24 provide an example of theoperation of stereo matching block 109.

With reference to FIG. 23A, an example of how the stereo matching ofpoints (such as feature points) may be achieved as follows. For eachcable point in the left image, find its corresponding point in the rightimage. Examples of how this may be done are provided above, particularlyin the discussion of the SIFT algorithm. Other correspondence matchingalgorithms may also be used. After rectification, the correspondingpoints in the left and right images lie in the same horizontal line.Feature base descriptors are also computed. For example, the ORB-basedfeature descriptor may be computed at the left-image pixel, and one canlimit the search for a corresponding pixel in the right image to thehorizontal line that passes through the pixel being considered in theleft image. That is, one can search the right-image pixels in the samerow to find the best match with the closest feature values. This isillustrated in FIG. 23A by a multiple illustrated horizontal lines alongcorresponding (i.e., matched) left and right pixels.

Preferably, outliers are systematically rejected. This can be achievedby using global geometry constraint to reject pixels (i.e., points) withdisparities out of range.

Triangulation block 109 b then defines the 3D position for the points todefine a perspective representation of a cable using the matched pointsin the left and right images. This may be achieved by using thehomography/epipolar/triangulation and/or other techniques discussedabove. For example, FIG. 23A shows the triangulation of a pair ofcorresponding pixels in left and right images determines a cable pointin the 3D space.

This triangulation operation may include the following. For each cablepoint, the 3D position is determined by the intersection of two lines,one connecting its left-image pixel and the left camera optical center,and another connecting the corresponding right-image pixel and the rightcamera center. For example with reference to FIG. 23B, let (x,y,z) bethe 3D point, and (u,v) and (u′,v′) denote the corresponding pixels inthe left and right image, and let 3-by-4 matrices P and P′ denote thecamera matrices for the left and right cameras. Then Pi denotes the i-throw P and Pi′ denotes i-th row of P′. One can then use the cameraprojection equation to result in the equation of FIG. 23B.

As an added example, FIG. 24 illustrates a reconstructed 3D cablegeometry shown in two different views.

The thus defined 3D point cloud is passed to 3D pose estimation block115, which estimates the pose of each part (preferably each point) inthe point cloud that defines a cable. In so doing, 3D pose estimationblock 115 also estimates the pose of different parts of the imagedcable. A preferred method to achieve this is to determine the surfacenormal at each point in the point cloud. The surface normal estimationmay be implemented as follows. At each cable point, find all cablepoints in a local window, of preferably 60 pixels. Use the estimated 3Dcable points in the local window to fit a plane onto the window, andthen estimate the local normal direction to that plane.

The estimating of surface normals at points on a cable is illustrated inFIG. 25. The surface normals estimated using the 3D cable point clouds(two views) at specified locations are illustrated by small arrows ofvarious arrow head sizes.

The pose information then passes to configurable structurerepresentation block 117, one of whose objective is to identify nodes(or node markers), cable segments (or cable units), and connectors. Thenodes preferably define the opposing ends of (non-rigid) cable segments(i.e., cable units), and each (non-rigid) cable segment is joined to aconnector or to another (non-rigid) cable segment at a node. Stated moresimply, a cable harness consists of a number of cable units, connectors,and nodes. Nodes are defined as the ends of the cable units. Eachconnector is attached with one end (node) of a cable unit. An example ofa cable harness consisting of 3 cable units, 4 nodes, and 3 connectorsis illustrated in FIG. 26.

The cable unit is preferably defined by a continuous chain of primitiveshapes, which can be cylinders or rectangular prisms. The primitiveshapes function as bounding envelops of the local cable unit areas. Itis thus important to estimate them from images for robotic operations oncables. Connector may be defined by rectangular prisms or by associatedCAD data. A node may be defined by a 3D point. FIG. 27 is an example ofround cable units represented by cylinders, as the primitive geometricshape. FIG. 28 is an example of flat cable units represented byrectangular prisms, as the primitive geometric shape.

The representation of cable harnesses may be implemented as follows: Ateach cable unit, a set of cable points are needed as centers ofprimitive shapes. These center points may be manually specified in theimage or automatically calculated by uniform division of thereconstructed cable unit points. At each center point, a primitive shapeis fitted with the reconstructed cable points in the local area anddetermines its size. For example, for a cylinder, a diameter and lengthmay be determined. Similarly, for a rectangular prism, its height andwidth may be determined. These sizes are stored so that one can movealong a cable unit by counting its representative geometric shapes. Ifdesired, the geometric shapes may be of a fixed (predefined) lengths.For examples, all the cylinders may have a first common length and/orall the rectangular prisms may have a second common length. If desired,the first and second common lengths may be the same.

The 3D orientation of the local primitive shapes also needs to bedetermined, especially for the flat cable units. A sequence of estimated3D cable primitives (cylinders) using the 3D cable point clouds andsurface normals (two views) is illustrated in FIG. 29.

With reference to FIG. 30, geometric model fitting to estimate cableprimitives may be divided into several parts. First, one specifies ordetects cable center points, preferably by uniform division. Given thecable centers, the distance of neighboring centers decides the length oflocal primitive cylinders. One then projects the cable points into theestimated local surface plane and finds the area of the projections. Thecable points are then projected to the direction perpendicular to theconnecting line of labeled cable centers, and the range decides thediameter of the local primitive cylinder.

Thus, the function of primitive shape fitting block 119 may beillustrated by illustrated by FIG. 31. As is illustrated, thereconstructed cable primitives are fitted with the structurerepresentation model to obtain a concise interpretation of the cablegeometry.

With reference to FIG. 32, given the interpreted cable structure, arobot hand can be made to easily operate with the cable. For example, ifa robot hand is programmed to pick a cable at the second primitive ofCable Unit 2 between Node b and c, the present invention can providesufficient information to fulfill this operation.

Thus, the robot can be made to recognize and manipulate different (andpreviously unknown) types of cable harnesses in an assembly line. Thisis achieve by a 3D cloud of a given cable harness. Pose information ofspecific parts of the cable harness are determined from the 3D pointcloud, and the cable harness is then re-presented as a collection ofprimitive geometric shapes of known dimensions, whose positions followthe spatial position of the re-presented cable harness. Because thenormal direction at each surface point of the harness is known, ifdesired, the orientations of the primitive geometric shapes may also bemade to follow the changes in orientation of the represented cableharness. The robot can then manipulate the cable harness by using thesimplified representation as a reference.

While the invention has been described in conjunction with severalspecific embodiments, it is evident to those skilled in the art thatmany further alternatives, modifications and variations will be apparentin light of the foregoing description. Thus, the invention describedherein is intended to embrace all such alternatives, modifications,applications and variations as may fall within the spirit and scope ofthe appended claims.

What is claimed is:
 1. A cable harness visualization system, comprising:a three-dimensional, i.e., 3D, imaging system imaging at least onephysical cable harness and creating a 3D cable harness model of theimaged physical cable harness; a cable structure representation modulerepresenting said 3D cable harness model in terms of sets of predefinedprimitive 3D shapes and node markers, each primitive 3D geometric shapebeing of known dimensions, the imaged physical cable harness havingconnectors and non-rigid cable segments, said node markers defining theopposing ends of said non-rigid cable segments, each non-rigid cablesegment being joined to a connector or to another non-rigid cablesegment at a node marker; wherein: each non-rigid cable segment isrepresented by a corresponding one of said sets of predefined primitive3D shapes; the corresponding set of predefined primitive 3D shapesconsisting of a train of primitive 3D geometric shapes that spatiallytrack the path of its corresponding non-rigid cable segment, eachnon-rigid cable segment extends into the interior of its correspondingtrain of geometric shapes, and the individual geometric shapes within atrain of geometric shapes turn in accord with twists and turns of thenon-rigid cable segment they represent; wherein in said cable structurerepresentation module, IF a non-rigid cable segment consists of aplurality of wires substantially coupled linearly side-by-side eachother to a connector, THEN its corresponding train of primitive 3Dgeometric shapes consists of a train of rectangular prisms; ELSE itscorresponding train of primitive 3D geometric shapes consists of a trainof cylinders.
 2. The cable harness visualization system of claim 1,wherein the widths of said rectangular prisms substantially span acrossthe side-by-side wires they represent, and the diameters of saidcylinders substantially encompass the portion of the non-rigid cablesegment they represent.
 3. The cable harness visualization system ofclaim 1, wherein all rectangular prisms within the train of rectangularprisms have a common first length, and all cylinders within train ofcylinders have a common second length.
 4. The cable harnessvisualization system of claim 1, wherein each non-rigid cable segment ischaracterized by a lack of joints or rigid portions between its opposingends.
 5. A cable harness visualization system, comprising: athree-dimensional, i.e., 3D, imaging system imaging at least onephysical cable harness and creating a 3D cable harness model of theimaged physical cable harness; a cable structure representation modulerepresenting said 3D cable harness model in terms of sets of predefinedprimitive 3D shapes and node markers, each primitive 3D geometric shapebeing of known dimensions, the imaged physical cable harness havingconnectors and non-rigid cable segments, said node markers defining theopposing ends of said non-rigid cable segments, each non-rigid cablesegment being joined to a connector or to another non-rigid cablesegment at a node marker; wherein: each non-rigid cable segment isrepresented by a corresponding one of said sets of predefined primitive3D shapes; the corresponding set of predefined primitive 3D shapesconsisting of a train of primitive 3D geometric shapes that spatiallytrack the path of its corresponding non-rigid cable segment, eachnon-rigid cable segment extends into the interior of its correspondingtrain of geometric shapes, and the individual geometric shapes within atrain of geometric shapes turn in accord with twists and turns of thenon-rigid cable segment they represent: wherein said 3D imaging systemincludes: a stereo imaging device producing stereoscopic image pairs ofsaid physical cable harnesses, each stereoscopic image pair including afirst image and a second image; a two-dimensional, 2D, cablelocalization module localizing commonly imaged physical cable harnessesin said first and second images, said 2D cable localization modulefurther identifying corresponding pixel pairs in said first and secondimages, each corresponding pixel pair including a first pixel from saidfirst image and a second pixel from said second image, both first andsecond pixels corresponding to a commonly imaged point on a commonlyimaged physical cable harness; a 3D cable localization module creating a3D point cloud representation of each commonly imaged physical cableharness in 3D space in accordance with perspective constraints and thecommonly imaged physical cable harness' corresponding pixel pairs; a 3Dpose estimator determining cable pose orientations of commonly imagedphysical cable harnesses in said 3D space in accordance with theircorresponding 3D point clouds, said 3D pose estimator determining asurface normal direction for selected points within said 3D point cloudsrelative to a neighborhood-of-points of predefined size surrounding eachselected point, said 3D cable harness model being defined by acorresponding 3D point cloud and corresponding cable pose orientations.6. The cable harness visualization system of claim 5, wherein said 3Dpose estimator determines the surface normal direction for all pointswithin all 3D point clouds.
 7. The cable harness visualization system ofclaim 5, wherein said 2D cable localization module segments said firstand second image to define image segments, and each image segmentdefines a silhouette of an imaged cable harness.
 8. The cable harnessvisualization system of claim 7, wherein each image segment is definedby: (I) for each unsegmented part of an image, selecting a seed pointwithin the unselected part, and for each selected point iterativelyapplying the following steps: (i) determine similarity measures for thecolor similarities between the seed point and its nearest neighborpixels, said nearest neighbor pixels being candidate pixels; (ii) jointhe seed point and the candidate pixels if the similarity measures arehigher than a predefined threshold, and IF any of the joined pixels area part of an existing image segment, THEN the existing image segment isgrown to include the joined pixels, ELSE the joined pixels define a newimage segment; (iii) determine a region color distribution for the imagesegment of step (ii) and calculate its principle component; (II)returning to step (I) until all points within an image have beenselected, in turn.
 9. The cable harness visualization system of claim 8,further including after step (II): (III) joining together any proximateimage segments having a region color distribution similarity within apredefined first threshold and a geometric properties similarity withina predefined second threshold.
 10. The cable harness visualizationsystem of claim 9, wherein proximate image segments are defined as imagesegments separated by not more than 60 pixels.
 11. The cable harnessvisualization system of claim 9, wherein the region color distributionsimilarity and the geometric properties similarity between imagesegments are determined from their respective principle components,within predefined thresholds.
 12. The cable harness visualization systemof claim 8, further including after step (II): (a) determining a firstset of feature descriptors for the image segments in said first imageand a second set of feature descriptors for image segments in saidsecond image; and (b) defining said corresponding pixel pairs bymatching pairs of corresponding feature descriptors between the firstand second sets of feature descriptors.
 13. The cable harnessvisualization system of claim 12, wherein step (b) includes: identifyingas a candidate matching descriptor, a feature descriptor in said secondset that matches a given feature descriptor in said first set; and IFits relative position within said second image differs from the relativeposition of the given feature descriptor in said first image by morethan a predefined margin, THEN discarding said candidate matchingdescriptor, ELSE deeming said candidate matching descriptor as acorresponding pixel pair to the given feature descriptor.
 14. The cableharness visualization system of claim 12, wherein step (b) featuredescriptors in said first and second sets of feature descriptors arematched by means of a tree-based feature matching scheme.
 15. The cableharness visualization system of claim 5, wherein said 3D pose estimatorimplements the following steps: (a) applying global homography to saidfirst and second images to reject corresponding pixel pairs in saidfirst and second images that do not satisfy global homographyconstraints; and (b) for each corresponding pixel pair not rejected instep (a), applying local homography in accordance to its correspondingimage segments to further remove from each image segment any points thatmeeting local homography constraints.
 16. The cable harnessvisualization system of claim 5, wherein said 3D pose estimatordetermines a surface normal direction for a selected point by; defininga local window around the selected point; identifying the 3D pointswithin the defined local window; fitting a 2D plane on to the identified3D points, and estimating the local 3D normal direction of the fitted 2Dplane.
 17. A robotic system for manipulating cable harnesses, saidrobotic system implementing the cable harness visualization system ofclaim
 1. 18. A robotic system for manipulating cable harnesses, saidrobotic system implementing the cable harness visualization system ofclaim 5.